#include <iostream>
#include <fstream>
#include "control.pb.h" //包含protobuf的头文件
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>
#include <vector>

#include <pthread.h>
#include "driver_usart.h"
#include "driver_Xbox_Joystick.h"
#include <unistd.h>

//#include "testopencv.h"

using namespace std;

/**
 * 功能：监视手柄
 *
 */
xbox_map_t joystickMap;
void *monitorJoyStick(void *ptr)
{
    JoyStickDriver JoyStick; // 手柄控制驱动对象
    int len;

    JoyStick.JoyStickInit();
    memset(&joystickMap, 0, sizeof(xbox_map_t));
    std::cout <<"test"<< std::endl; // 打印数据内容
    while(1)
    {
        len = JoyStick.JoyStickRead(&joystickMap);
        if (len < 0)
        {
            usleep(10*1000);
            continue;
        }
        printf("\rTime:%8d A:%d B:%d X:%d Y:%d LB:%d RB:%d select:%d start:%d lo:%d ro:%d XX:%-6d YY:%-6d LX:%-6d LY:%-6d RX:%-6d RY:%-6d LT:%-6d RT:%-6d",
                joystickMap.time, joystickMap.a, joystickMap.b, joystickMap.x, joystickMap.y, joystickMap.lb, joystickMap.rb, joystickMap.select, joystickMap.start, joystickMap.lo, joystickMap.ro,
                joystickMap.xx, joystickMap.yy, joystickMap.lx, joystickMap.ly, joystickMap.rx, joystickMap.ry, joystickMap.lt, joystickMap.rt);
        //fflush(stdout);
    }

    JoyStick.JoyStickClose();
    return 0;
}

/**
bool SerializeToString(string* output) const;把message编码进output。
bool ParseFromString(const string& data);从string解码到message
bool SerializeToArray(char* buf,int size) const;把message编码进数组buf.
bool ParseFromArray(const char* buf,int size);把buf解码到message。
此解码方法效率较ParseFromString高很多，所以一般用这种方法解码，而且IO函数一般是以char*交流的，不是string。
bool SerializeToOstream(ostream* output) const;把message编码进ostream
bool ParseFromIstream(istream* input);从istream解码到message
*/
int main(int argc, char *argv[])
{
    XGrobot::MoveCmd moveCmd;     // 一个储存protobuf数据的实例

    uint8_t proto_buffer[40]; // 根据数据的长度定义一个数组
    uint8_t serial_buffer[50]; // 根据数据的长度定义一个数组
    pthread_t id;
    pthread_t id2;
    std::cout <<"test begin"<< std::endl; // 打印数据内容

    uint8_t videoFlag = 0;

    SerialDriver serial; // 串口驱动对象，包含了对串口的初始化
    int value = 0;
    int angle = 0;
    int ret = pthread_create(&id, NULL, monitorJoyStick, NULL);
    if(ret) {
        cout << "Create pthread error!" << endl;
        return 1;
    }

    serial_buffer[0] = 1; // 串口发送数据的第一位是数据类型

    while(1)
    {
        //sleep(10);
        usleep(1000*50); //1ms = 1000us
        /*数据发送：
            将手柄的int数据转换成线速度和角速度，通信时直接发送线速度和角速度
            线速度范围正负一百 手柄实际输出±32768
            角速度范围正负一百 手柄实际输出±32768
        */



        //moveCmd.set_linear(joystickMap.ly*100/32768);    // 给一个数据的元素赋值
        //moveCmd.set_angular(joystickMap.lt*100/32768);
        if(joystickMap.start == 1) {
            value = joystickMap.ly*100/32768;
        }
        if(joystickMap.lb == 1) {
            value = -50;
        }
        if(joystickMap.rb == 1) {
            angle = -25;
        }

        if(joystickMap.x == 1) {
            angle--;
        }
        if(joystickMap.b == 1) {
            angle++;
        }
        if(joystickMap.y == 1) {
            value--;
        }
        if(joystickMap.a == 1) {
            value++;
        }
   /*     if(joystickMap.select == 1 && videoFlag == 0) {
   //         exitVideo(ON);
   //         int ret2 = pthread_create(&id2, NULL, opencvVideo, NULL);
   //         if(ret2) {
   //             cout << "Create pthread error!" << endl;
   //             return 1;
            }
   //         videoFlag = 1;
        }
        if(joystickMap.start == 1 && videoFlag == 1) {
            exitVideo (OFF);
            videoFlag = 0;
        }*/

        if(joystickMap.yy < -1) {
            if(joystickMap.xx < -1) {
                moveCmd.set_linear(value);    // 给一个数据的元素赋值
                moveCmd.set_angular(angle);
                printf("%d %d\n", value , angle);
            } else if(joystickMap.xx > 1) {
                moveCmd.set_linear(value);    // 给一个数据的元素赋值
                moveCmd.set_angular(-angle);
                printf("%d %d\n", value , -angle);
            } else {
                moveCmd.set_linear(value);    // 给一个数据的元素赋值
                moveCmd.set_angular(joystickMap.rx*100/32768);
                printf("%d %d\n", value , angle);
            }

        } else if(joystickMap.yy > 1 ) {
            if(joystickMap.xx < -1) {
                moveCmd.set_linear(-value);    // 给一个数据的元素赋值
                moveCmd.set_angular(angle);
                printf("%d %d\n", -value , angle);
            } else if(joystickMap.xx > 1) {
                moveCmd.set_linear(-value);    // 给一个数据的元素赋值
                moveCmd.set_angular(-angle);
                printf("%d %d\n", -value , -angle);
            } else {
                moveCmd.set_linear(-value);    // 给一个数据的元素赋值
                moveCmd.set_angular(joystickMap.rx*100/32768);
                printf("%d %d\n", -value , angle);
            }
        } else {
            moveCmd.set_linear(joystickMap.ly*100/32768);    // 给一个数据的元素赋值
            moveCmd.set_angular(joystickMap.lt*100/32768);///////////////////////////////////////////////////////////////////////////////rx->lt
            printf("%d %d %d\n", joystickMap.ly*100/32768 , joystickMap.lt*100/32768, videoFlag);///////////////////////////////////////////////rx->lt
        }



        //const int proto_len = moveCmd.ByteSizeLong(); // 获取数据的长度
        serial_buffer[1] = moveCmd.ByteSizeLong();
        moveCmd.SerializeToArray(proto_buffer, serial_buffer[1]); // 讲数据通过编码写入到buf内
        for(int i=0; i<40; i++) {
            serial_buffer[i+2] = proto_buffer[i];
        }

        serial.Write(serial_buffer, 30); // 通过串口将数据发送给单片机
        memset(serial_buffer, 0, sizeof(serial_buffer));

    }
/*
    // 数据接收
    while(1)
    {
        if(REV_TRUE == serial.ReadBytes(proto_buffer, sizeof(proto_buffer))) // 通过阻塞方式接收串口数据中断
            break;
    }
    moveCmd_REV.ParseFromArray(proto_buffer, proto_len); // 将接收到的串口数据重新解析成为protobuf数据
    std::cout <<"linear: "<< moveCmd_REV.linear() << std::endl; // 打印数据内容
    std::cout <<"angular: "<< moveCmd_REV.angular() << std::endl;*/
    return 0;
}


